//
// Created by 24087 on 2025/4/27.
//

#include "Interface_ROS.h"
#include "Interface_Motor.h"
uint32_t uniqueID_H = 0;
uint32_t uniqueID_L = 0;
Queue1 PairPool;

/**
 * @brief 解析ROS数据帧
 *
 * 该函数用于解析传入的ROS数据帧。首先跳过帧头，然后逐字节处理数据，
 * 将高字节和低字节组合为16位整数，并将其转换为浮点值，最后打印解析结果。
 *
 * @param data_H 指向高字节数据数组的指针
 * @param data_L 指向低字节数据数组的指针
 *
 * 注意：该函数假设输入数据的有效性，并依赖于数据的正确对齐和长度。
 */
float *Parser_ROS(const uint8_t *data_H, const uint8_t *data_L)
{
	/*跳过帧头*/
	data_H++;
	data_L++;

	float *targetAngle = malloc(sizeof(float) * 6);
	/*处理数据*/
	for (uint8_t i = 0; i < 6; ++data_H, ++data_L, ++i)
	{
		const float val = (int16_t) (*data_H << 8 | *data_L) * 0.001f;
		targetAngle[i] = val;
	}

	return targetAngle;
}

// 定义所有Data_H和Data_L包
const uint8_t data_H[8] = {0x05, 0x04, 0x0c, 0xfa, 0x06, 0xf6, 0x01, 0xFF};
const uint8_t data_L[8] = {0x05, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};

const uint8_t data_angle[8] = {0x01, 0x36, 0x01, 0x00, 0x01, 0x00, 0x00, 0x6B};

// 提取帧头和帧尾检查逻辑为独立函数
static bool IsValidFrame(const uint8_t *data)
{
	return data[0] == ROS_FrameHeader &&
	       (data[7] == ROS_FrameTail_H || data[7] == ROS_FrameTail_L);
}

/**
 * @brief 检查ROS数据帧的合法性
 *
 * 该函数用于验证传入的数据帧是否符合ROS协议的帧头帧尾要求，并进行空指针检查。
 *
 * @param data 指向待检查数据帧的指针，数据帧应为字节数组形式
 * @return HAL_StatusTypeDef 返回操作状态。如果数据帧合法且通过检查，返回 HAL_OK；
 *         如果数据帧为空或未通过帧头帧尾检查，返回 HAL_ERROR。
 */
HAL_StatusTypeDef1 ROS_Check(const uint8_t *data)
{
	// 空指针检查
	if (data == NULL)
	{
		return HAL_ERROR1;
	}

	// 帧头帧尾检查
	return IsValidFrame(data) ? HAL_OK1 : HAL_ERROR1;
}

/**
 * @brief 根据数据帧尾标识将数据分类并分配唯一ID后推入队列
 *
 * 该函数检查输入数据的格式，根据帧尾标识确定数据类型（高字节帧或低字节帧），
 * 分配唯一的ID，并将数据推入指定的队列中。如果数据格式不匹配，则输出错误信息。
 *
 * @param data 指向输入数据的指针，数据格式应符合ROS帧协议
 * @param PairPool 指向目标队列的指针，用于存储处理后的数据对
 */
void BufferMov_ROS(const uint8_t *data, Queue1 *PairPool)
{
	/*检查数据格式，确定data类型，并打上唯一ID*/
	switch (data[7])
	{
		/*处理高字节帧*/
		case ROS_FrameTail_H:
			bool isHighByte = true;
			uniqueID_H++;
			Pairpool_Push(PairPool, uniqueID_H, data, isHighByte);
			break;
		/*处理低字节帧*/
		case ROS_FrameTail_L:
			isHighByte = false;
			uniqueID_L++;
			Pairpool_Push(PairPool, uniqueID_L, data, isHighByte);
			break;
		default:
			printf("Wrong data!!!");
	}
}

void Pairpool_Init(Queue1 *PairPool)
{
	Queue1Init(PairPool);
}

void Pairpool_Destroy(Queue1 *PairPool)
{
	Queue1Destroy(PairPool);
}

/**
 *
 */
void Pairpool_Push(Queue1 *PairPool, const uint32_t ID, const uint8_t *rawdata, const bool isHighByte)
{
	/*有未配对pair则配对*/
	for (int i = 0; i < BUFFER_SIZE; ++i)
	{
		/*有未配对高字节包*/
		if (ID == PairPool->data[i].uniqueID && isHighByte)
		{
			for (int j = 0; j < BUFFER_SIZE; ++j)
			{
				PairPool->data[i].data_H[j] = rawdata[j];
			}
			PairPool->data[i].isComplete = true;

			return;
		}

		/*有未配对低字节包*/
		if (ID == PairPool->data[i].uniqueID && !isHighByte)
		{
			for (int j = 0; j < BUFFER_SIZE; ++j)
			{
				PairPool->data[i].data_L[j] = rawdata[j];
			}
			PairPool->data[i].isComplete = true;

			return;
		}
	}

	/*无未配对pair则新建并插入*/
	ROS_Pair pair_temp = {.data_H = {0}, .data_L = {0}, .uniqueID = -1, .isComplete = false};
	/*处理高字节数据*/
	if (isHighByte)
	{
		/*设置唯一ID*/
		pair_temp.uniqueID = uniqueID_H;

		/*设置高字节数据*/
		for (int i = 0; i < BUFFER_SIZE; ++i)
		{
			pair_temp.data_H[i] = rawdata[i];
		}

		/*标记未配对*/
		pair_temp.isComplete = false;
		Queue1Push(PairPool, pair_temp);
	}

	if (!isHighByte)
	{
		/*设置唯一ID*/
		pair_temp.uniqueID = uniqueID_L;

		/*设置低字节数据*/
		for (int i = 0; i < BUFFER_SIZE; ++i)
		{
			pair_temp.data_L[i] = rawdata[i];
		}

		/*标记未配对*/
		pair_temp.isComplete = false;
		Queue1Push(PairPool, pair_temp);
	}
}

bool Pairpool_Empty(const Queue1 *PairPool)
{
	return Queue1Empty(PairPool);
}

QE_DataType1 Pairpool_Pop(Queue1 *PairPool)
{
	return Queue1Pop(PairPool);
}

/*
 *
 */
void ROS_Process(Queue1 *PairPool, const MotorInfo *MotorPool)
{
	/*从非空缓存区中拿取数据对*/
	/*缓存区判空*/
	if (Pairpool_Empty(PairPool))
	{
		printf("PairPool is empty!!!");
		return;
	}
	/*遍历pair池数据对，数据对判整，数据对完整则解算，否则退出*/
	float *targetAngle = NULL;
	for (int i = 0; i < BUFFER_SIZE; ++i)
	{
		if (PairPool->data[i].isComplete)
		{
			const ROS_Pair pair = Pairpool_Pop(PairPool);
			/*解码数据为目标角度*/
			targetAngle = Parser_ROS(pair.data_H, pair.data_L);
			/*拿到对应关节电机的控制信号*/
			TargetInfo *target = ROS_TimeUnify(targetAngle, MotorPool);
			/*发送*/
			ROS_Send(target);
			/*释放空间*/
			free(targetAngle);
			targetAngle = NULL;
		}
	}
}

TargetInfo *ROS_TimeUnify(float *targetAngle, const MotorInfo *MotorPool)
{
	const float PI = 3.14159265358979323846f;
	const float RUNTIME = 3000.0f;
	TargetInfo *targetInfo = malloc(sizeof(TargetInfo) * 6);

	for (int i = 0; i < Motor_NUM; ++i)
	{
		/*目标角度转换：float -> hex  &  目标方向标定*/
		const uint8_t t_dir = targetAngle[i] > 0 ? 0 : 1;
		targetAngle[i] = targetAngle[i] > 0 ? targetAngle[i] : -targetAngle[i];
		const uint32_t t_clk = (uint32_t) roundf(targetAngle[i] / (2.0f * PI) * 3200.0f);
		const uint16_t t_vel = (uint16_t) (t_clk / (uint32_t) roundf(16.0f * RUNTIME / 1000.0f));
		/*将方向、速度、加速度、脉冲数封装为TargetInfo*/
		const TargetInfo temp = {.dir = t_dir, .vel = t_vel, .acc = 125, .clk = t_clk};
		targetInfo[i] = temp;
	}

	return targetInfo;
}

void ROS_Send(TargetInfo *targetInfo)
{
	for (int i = 0; i < Motor_NUM; ++i)
	{
		printf("ID -> %d Send Down!!!\n", i);
	}
}
